#include <dim-sum/linkage.h>
#include <dim-sum/sched.h>
#include <dim-sum/stringify.h>
#include <dim-sum/process.h>
#include <dim-sum/boot_allotter.h>
#include <dim-sum/beehive.h>
#include <dim-sum/mem.h>
#include <dim-sum/errno.h>
#include <dim-sum/mmu.h>
#include <dim-sum/exec.h>
#include <uapi/asm/ptrace.h>
#include <asm/asm-offsets.h>
#include <asm/head64.h>
#include <asm/sysreg.h>
#include <asm/processor.h>
#include <asm/exception.h>

static inline void inv_all_tlb(void)
{
	asm volatile(
		"tlbi vmalle1is \n\t"
		:::"memory" 
	);
}

/**
 * 默认的idle节能调用:WFI
 */
void cpu_do_idle(void)
{
	asm volatile(
		"dsb sy \n\t"
		"wfi \n\t"
	);
	return;
}

void arch_init_kernel_regs(struct task_desc * tsk, unsigned long stack, unsigned long argv, unsigned long envp)
{
	tsk->fp_regs = (struct fp_context*)kzalloc(sizeof(struct fp_context), PAF_ATOMIC);
	if (!tsk->fp_regs)
		return;
	memset(&tsk->task_spot, 0x0, sizeof(struct task_spot));
	tsk->task_spot.cpu_context.x19 = argv;
	tsk->task_spot.cpu_context.x20 = envp;
	tsk->task_spot.cpu_context.fp = 0;
	/**
	 * 这里的魔数 0x10,是因为 __switch_cpu_context 在返回时
	 *	0xffffffc0000a99f0 <__switch_cpu_context+92>:	1f 20 03 d5	nop
	 * 	0xffffffc0000a99f4 <__switch_cpu_context+96>:	ff 43 00 91	add	sp, sp, #0x10
	 * 	0xffffffc0000a99f8 <__switch_cpu_context+100>:	c0 03 5f d6	ret
	 * 存在一个 add	sp, sp, #0x10 的动作。
	 * 为了保证在 __switch_cpu_context 切到 task_entry 时是正常的,这里提前加 0x10
	 */
	tsk->task_spot.cpu_context.sp = stack + THREAD_START_SP - 0x10;
	tsk->task_spot.cpu_context.pc = (unsigned long)(&task_entry);
}

void arch_init_user_regs(struct task_desc * tsk, unsigned long stack)
{
	BUG_ON(!tsk || !stack);
	tsk->user_regs = (struct exception_spot*)(stack + THREAD_START_SP - S_FRAME_SIZE);
}

char *arch_get_argv(void)
{
	return (char*)(current->task_spot.cpu_context.x19);
}

char *arch_get_envp(void)
{
	return (char*)(current->task_spot.cpu_context.x20);
}

/**
 * 线程切换时，保存线程的寄存器并恢复新线程的寄存器
 */
asmlinkage struct task_desc* __switch_cpu_context(struct task_desc *prev,
	struct task_desc *next)
{

	asm volatile(
		"add x8, %[PREV],#" __stringify(THREAD_CPU_CONTEXT) "\n\t"
		"mov	x9, sp \n\t"
		"stp	x19, x20, [x8], #16 \n\t"
		"stp	x21, x22, [x8], #16 \n\t"
		"stp	x23, x24, [x8], #16 \n\t"
		"stp	x25, x26, [x8], #16 \n\t"
		"stp	x27, x28, [x8], #16 \n\t"
		"stp	x29, x9, [x8], #16 \n\t"
		"str	x30, [x8] \n\t"
		"add	x8, %[NEXT], #" __stringify(THREAD_CPU_CONTEXT) "\n\t"
		"ldp	x19, x20, [x8], #16 \n\t"
		"ldp	x21, x22, [x8], #16 \n\t"
		"ldp	x23, x24, [x8], #16 \n\t"
		"ldp	x25, x26, [x8], #16 \n\t"
		"ldp	x27, x28, [x8], #16 \n\t"
		"ldp	x29, x9, [x8], #16 \n\t"
		"ldr	x30, [x8] \n\t"
		"mov	sp, x9 \n\t"		
		:
		: [PREV]"r"(prev), [NEXT]"r"(next)
		: "memory"
	);
	return prev;
}

/**
 * 切换进程的fpsimd上下文
 */
asmlinkage void __switch_fpsimd_context(struct task_desc *prev,
	struct task_desc *next)
{
	save_fp_regs(prev);
	restore_fp_regs(next);
}

/**
 * 切换进程的MMU页表上下文
 */
asmlinkage void __switch_mmu_context(struct process_desc *prev,
	struct process_desc *new)
{	
	/**
	 * 注意并不是每个进程都有mem_desc结构
	 * 比如init_idle_process函数建立的进程
	 */
	if (!new || !new->mem)
		return;
	
	mmu_set_tlb(&new->mem->mem_arch.mmu);
	return;
}


void start_user_task(struct task_desc *task)
{
	BUG_ON(!task);
	BUG_ON(!task->user_regs);
	start_user_run((u64)task->user_regs);
}

void init_user_context(struct exception_spot* s)
{
	struct task_desc *task = current;
	if (!task || !task->user_regs)
		BUG();

	BUG_ON(s != task->user_regs);
	memset(s, 0, sizeof(*s));
	if (task->user_exec_info && 
			task->user_exec_info->user_argenv_sp) {
		/**
		 * 有命令行参数和环境变量的情况
		 */
		s->sp = (u64)(task->user_exec_info->user_argenv_sp);

	} else {
		/**
		 * 没有有命令行参数和环境变量或者处理参数失败的情况
		 */
		s->sp = (u64)(mem_stack_top(current_mem) & PAGE_MASK);
	}

	s->pc = (u64)task->task_main;
	s->pstate = PSR_MODE_EL0t;
	return;
}


void save_fp_regs(struct task_desc *task)
{
	
	if (!task || !task->fp_regs)
		return;

	asm volatile(

		/**
		 * 保存浮点控制寄存器
		 */
		"mrs	x1, fpcr\n\t" /* fpcr */
		"str	w1, [%0, #" __stringify(FP_FPCR) "]\n\t"
		"mrs	x1, fpsr\n\t" /* fpsr */
		"str	w1, [%0, #" __stringify(FP_FPSR) "]\n\t"

		/**
		 * 保存浮点寄存器
		 */
		"add	x1, %0, #" __stringify(FP_FPREGS) "\n\t"
		"stp	 q0,  q1, [x1, #0x000]\n\t"
		"stp	 q2,  q3, [x1, #0x020]\n\t"
		"stp	 q4,  q5, [x1, #0x040]\n\t"
		"stp	 q6,  q7, [x1, #0x060]\n\t"
		"stp	 q8,  q9, [x1, #0x080]\n\t"
		"stp	q10, q11, [x1, #0x0A0]\n\t"
		"stp	q12, q13, [x1, #0x0C0]\n\t"
		"stp	q14, q15, [x1, #0x0E0]\n\t"
		"stp	q16, q17, [x1, #0x100]\n\t"
		"stp	q18, q19, [x1, #0x120]\n\t"
		"stp	q20, q21, [x1, #0x140]\n\t"
		"stp	q22, q23, [x1, #0x160]\n\t"
		"stp	q24, q25, [x1, #0x180]\n\t"
		"stp	q26, q27, [x1, #0x1A0]\n\t"
		"stp	q28, q29, [x1, #0x1C0]\n\t"
		"stp	q30, q31, [x1, #0x1E0]\n\t"

		:
		: "r"(task->fp_regs)
		: "x1", "memory");
	return;
}

void restore_fp_regs(struct task_desc *task)
{
	if (!task || !task->fp_regs) {
		return;
	}

	asm volatile(

		/**
		 * 恢复浮点寄存器
		 */
		"add	x1, %0, #" __stringify(FP_FPREGS) "\n\t"
		"ldp	 q0,  q1, [x1, #0x000]\n\t"
		"ldp	 q2,  q3, [x1, #0x020]\n\t"
		"ldp	 q4,  q5, [x1, #0x040]\n\t"
		"ldp	 q6,  q7, [x1, #0x060]\n\t"
		"ldp	 q8,  q9, [x1, #0x080]\n\t"
		"ldp	q10, q11, [x1, #0x0A0]\n\t"
		"ldp	q12, q13, [x1, #0x0C0]\n\t"
		"ldp	q14, q15, [x1, #0x0E0]\n\t"
		"ldp	q16, q17, [x1, #0x100]\n\t"
		"ldp	q18, q19, [x1, #0x120]\n\t"
		"ldp	q20, q21, [x1, #0x140]\n\t"
		"ldp	q22, q23, [x1, #0x160]\n\t"
		"ldp	q24, q25, [x1, #0x180]\n\t"
		"ldp	q26, q27, [x1, #0x1A0]\n\t"
		"ldp	q28, q29, [x1, #0x1C0]\n\t"
		"ldp	q30, q31, [x1, #0x1E0]\n\t"

		/**
		 * 恢复浮点控制寄存器
		 */
		"ldr	w1, [%0, #" __stringify(FP_FPSR) "]\n\t"
		"msr	fpsr, x1\n\t" /* fpsr */
		"ldr	w1, [%0, #" __stringify(FP_FPCR) "]\n\t"
		"msr	fpcr, x1\n\t" /* fpcr */
		:
		: "r"(task->fp_regs)
		: "x1", "memory");
	return;
}

void save_tls(struct task_desc *t)
{
	*(&(t->tp_value)) = read_sysreg(tpidr_el0);
}

void restore_tls(struct task_desc *t)
{
	unsigned long tpidr, tpidrro = 0;
	tpidr = *(&(t->tp_value));
	write_sysreg(tpidr, tpidr_el0);
	write_sysreg(tpidrro, tpidrro_el0);
}

/**
 * 切换进程的tls上下文
 */
asmlinkage void __switch_tls_context(struct task_desc *prev,
	struct task_desc *next)
{
	save_tls(prev);
	restore_tls(next);
	return;	
}